#!/usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain-basic
(rbrain)

;; ros init
(ros::roseus "kinect-openrave-graspplanning")

(ros::roseus-add-msgs "sensor_msgs")
(ros::roseus-add-msgs "object_manipulation_msgs")
(ros::roseus-add-srvs "object_manipulation_msgs")
(ros::roseus-add-srvs "orrosplanning")

(load "graspplanning-util.l")
(load "util-cds.l")

(load "irt-all-objects-pointcloud.l")
(load "package://jskpointcloud/euslisp/jskpointcloud.l")
(setup-roseus-services)

(defun pcloud-cb (msg)
  (let ((3dp (make-3dpointcloud-from-msg2 msg :remove-nan t)))
    (do-graspplanning 3dp)
    )
  )

(defun do-graspplanning (3dp &key (robot :hrp2jsknt) (grasparm :rarm))
  (set-robot robot)
  (set-grasp-parameters robot)
  (set-grasp-parameters-service-call)
  (setq *graspplanning-res*
        (graspplanning-service-call
         grasparm
         (create-GraspableObject-msg-from-pcloud
          3dp "map")))  
  )

(ros::subscribe "/points_out" sensor_msgs::PointCloud2 #'pcloud-cb)

(defun ros-proc ()
  (ros::rate 10)
  (ros::spin-once)
  )